﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Microsoft.Xna.Framework;
using Microsoft.Xna.Framework.Graphics;

using Drifting.Foundation;
using Drifting.Graphics.EffectObjects;
using Drifting.Graphics.Visualizers;
using Drifting.Graphics.PhysicalObjects.Vehicles;
using Drifting.MotionPlanning.MultiThreading.Planners;
using Drifting.MotionPlanning.Primitives;
using Drifting.MotionPlanning.Systems;

namespace Drifting.Scenes
{
    public class SST_Drift12D : Basic
    {
        SST sst;
        Atlantis atlantis;

        public SST_Drift12D(ref Matrix view, ref Matrix projection, GraphicsDevice device, Camera camera, Game1 game)
            : base(ref view, ref projection, device, camera, game)
        {
            camera.Camera_InitView(40, 10, 20, 0, 0, 0, (float)(Math.PI / 4), ref view, ref projection);

            atlantis = new Atlantis(game);
            atlantis.Scale = 0.08f;

            BaseSystem drift12D = new Drift12D();
            Skin_G visual = new Skin_G(new List<State>(), device);
            

            double[] q_zero = new double[] {-2, 0, 0, 1, 0, 0, 0 };
            sst = new SST(
                drift12D,
                q_zero,
                TextAssistant.LoadFile("../../../MotionPlanning/Resources/SST_Drift12D.kdl"),
                inXgoal,
                device,
                visual);

            sst.AgentNumber = 5;
            sst.Viewable = true;
            sst.Start(100000000); // 100 million
        }

        override public bool inXgoal(double[] q)
        {
            if (q[0] < 8) return false;
            if (Math.Abs(q[1]) + Math.Abs(q[2]) > 1) return false;

            double t0=0;
            double t1=1;
            double t2=0;
            double tw=Math.PI;

            double e0=0;
            double e1=0;
            double e2=0;
            double angle=0;
            MathAssistant.AxisAngleFromQuaternion(q[3], q[4], q[5], q[6], ref e0, ref e1, ref e2, ref angle);


            double dAxis = 0.2;
            float dAngle = 20;

            if (Math.Abs(angle - tw) > MathHelper.ToRadians(dAngle))
                return false;

            double d1 = e0 - t0;
            double d2 = e1 - t1;
            double d3 = e2 - t2;
            double dL2 = d1 * d1 + d2 * d2 + d3 * d3 ;


            if (dL2 > dAxis * dAxis)
                return false;




            return true;
        }

        override public Color BackgroundColor()
        {
            return Color.Black;
        }

        override public void Draw(Matrix camera, Matrix projection)
        {
            base.Draw(camera, projection);

            //sst.Skin.Draw(camera, projection, sst.x_solution);


            // draw solution
            if (sst.x_solution != null)
            {
                State x_i = sst.x_solution;
                while (x_i != null)
                {
                    double[] q = x_i.Q;
                    if (x_i.ParentEdge != null)
                    {
                        atlantis.Draw(camera, projection, (float)q[0], (float)q[1], (float)q[2], (float)q[3], (float)q[4], (float)q[5], (float)q[6]);
                    }
                                        
                    if (x_i.ParentEdge == null)
                        break;
                    x_i = x_i.ParentEdge.entry;
                }
            }

        }

        

        override public void Terminate()
        {
            sst.Terminate();
        }
    }
}
